#include "headers.h"

#define STILL_DETECT   			0
#define ORIENTATION_DETECT  1

detect_orientation_return detect_orientation(bool lenient_still_position){  ///< true: Use more lenient still position detection

	static float accel_ema[3] = { 0.0f };     // exponential moving average of accel
	static float	accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
	float	ema_len = 0.5f;				// EMA time constant in seconds
	const float	normal_still_thr = 0.25;		// normal still threshold
	float	still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
	float	accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
	uint64_t still_time = lenient_still_position ? 500000 : 1300000;	// still time required in us
	
	/* set timeout to 30s */
	uint64_t t = 0;
	static uint64_t t_prev = 0;	
	
	static uint32_t still_counter = 0;
	
	if(still_counter <= still_time / 10000){
		t = micros();
		float dt = (t - t_prev) / 1000000.0f;
		t_prev = t;
		float w = dt / ema_len;
			
		for(unsigned i = 0; i < 3; i++){
			float di = 0.0f;
			switch(i){
				case 0:
					di = imu.accRaw[0];
				break;
				case 1:
					di = imu.accRaw[1];
				break;
				case 2:
					di = imu.accRaw[2];
				break;
			}
				
			float d = di - accel_ema[i];
			accel_ema[i] += d * w;
			d = d * d;
			accel_disp[i] = accel_disp[i] * (1.0f - w);
				
			if (d > still_thr2 * 8.0f) {
					d = still_thr2 * 8.0f;
				}

				if (d > accel_disp[i]) {
					accel_disp[i] = d;
				}
		}
			
		/* still detector with hysteresis */
		if (accel_disp[0] < still_thr2 &&
					accel_disp[1] < still_thr2 &&
					accel_disp[2] < still_thr2) {
			/* is still now */
			still_counter++;
		}
		else if(accel_disp[0] > still_thr2 * 4.0f || accel_disp[1] > still_thr2 * 4.0f ||accel_disp[2] > still_thr2 * 4.0f){
			/* not still, reset still start time */
			still_counter = 0;
		}
		return DETECT_ORIENTATION_ERROR;
	}
	else{
		if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
				fabsf(accel_ema[1]) < accel_err_thr &&
				fabsf(accel_ema[2]) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_NOSE_UP;
		}
				
		if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
				fabsf(accel_ema[1]) < accel_err_thr &&
				fabsf(accel_ema[2]) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_NOSE_DOWN;
		}
		
		if (fabsf(accel_ema[0]) < accel_err_thr &&
				fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
				fabsf(accel_ema[2]) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_LEFT_SIDE_DOWN;        // [ 0, g, 0 ]
		}
				
		if (fabsf(accel_ema[0]) < accel_err_thr &&
				fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
				fabsf(accel_ema[2]) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_RIGHT_SIDE_DOWN;        // [ 0, -g, 0 ]
		}
				
		if (fabsf(accel_ema[0]) < accel_err_thr &&
				fabsf(accel_ema[1]) < accel_err_thr &&
				fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_ON_BACK;        // [ 0, 0, g ]
		}

		if (fabsf(accel_ema[0]) < accel_err_thr &&
				fabsf(accel_ema[1]) < accel_err_thr &&
				fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
					still_counter = 0;
			return DETECT_ORIENTATION_LEVEL;        // [ 0, 0, -g ]
		}
				
		return DETECT_ORIENTATION_ERROR;
	}
}

bool sphere_fit_least_squares(const float x[], const float y[], const float z[],
			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
			     float *sphere_radius){
	float x_sumplain = 0.0f;
	float x_sumsq = 0.0f;
	float x_sumcube = 0.0f;

	float y_sumplain = 0.0f;
	float y_sumsq = 0.0f;
	float y_sumcube = 0.0f;

	float z_sumplain = 0.0f;
	float z_sumsq = 0.0f;
	float z_sumcube = 0.0f;

	float xy_sum = 0.0f;
	float xz_sum = 0.0f;
	float yz_sum = 0.0f;

	float x2y_sum = 0.0f;
	float x2z_sum = 0.0f;
	float y2x_sum = 0.0f;
	float y2z_sum = 0.0f;
	float z2x_sum = 0.0f;
	float z2y_sum = 0.0f;
	
	for (unsigned int i = 0; i < size; i++) {

		float x2 = x[i] * x[i];
		float y2 = y[i] * y[i];
		float z2 = z[i] * z[i];

		x_sumplain += x[i];
		x_sumsq += x2;
		x_sumcube += x2 * x[i];

		y_sumplain += y[i];
		y_sumsq += y2;
		y_sumcube += y2 * y[i];

		z_sumplain += z[i];
		z_sumsq += z2;
		z_sumcube += z2 * z[i];

		xy_sum += x[i] * y[i];
		xz_sum += x[i] * z[i];
		yz_sum += y[i] * z[i];

		x2y_sum += x2 * y[i];
		x2z_sum += x2 * z[i];

		y2x_sum += y2 * x[i];
		y2z_sum += y2 * z[i];

		z2x_sum += z2 * x[i];
		z2y_sum += z2 * y[i];
	}
	
	//
	//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
	//
	//    P is a structure that has been computed with the data earlier.
	//    P.npoints is the number of elements; the length of X,Y,Z are identical.
	//    P's members are logically named.
	//
	//    X[n] is the x component of point n
	//    Y[n] is the y component of point n
	//    Z[n] is the z component of point n
	//
	//    A is the x coordiante of the sphere
	//    B is the y coordiante of the sphere
	//    C is the z coordiante of the sphere
	//    Rsq is the radius squared of the sphere.
	//
	//This method should converge; maybe 5-100 iterations or more.
	//
	float x_sum = x_sumplain / size;        //sum( X[n] )
	float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
	float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
	float y_sum = y_sumplain / size;        //sum( Y[n] )
	float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
	float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
	float z_sum = z_sumplain / size;        //sum( Z[n] )
	float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
	float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )

	float XY = xy_sum / size;        //sum( X[n] * Y[n] )
	float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
	float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
	float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
	float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
	float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
	float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
	float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
	float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
	
	//Reduction of multiplications
	float F0 = x_sum2 + y_sum2 + z_sum2;
	float F1 =  0.5f * F0;
	float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
	float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
	float F4 = -8.0f * (X2Z + Y2Z + z_sum3);

	//Set initial conditions:
	float A = x_sum;
	float B = y_sum;
	float C = z_sum;

	//First iteration computation:
	float A2 = A * A;
	float B2 = B * B;
	float C2 = C * C;
	float QS = A2 + B2 + C2;
	float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);

	//Set initial conditions:
	float Rsq = F0 + QB + QS;

	//First iteration computation:
	float Q0 = 0.5f * (QS - Rsq);
	float Q1 = F1 + Q0;
	float Q2 = 8.0f * (QS - Rsq + QB + F0);
	float aA, aB, aC, nA, nB, nC, dA, dB, dC;

	//Iterate N times, ignore stop condition.
	unsigned int n = 0;

	while (n < max_iterations) {
		n++;

		//Compute denominator:
		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
		aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
		aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
		aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;

		//Compute next iteration
		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
		nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
		nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);

		//Check for stop condition
		dA = (nA - A);
		dB = (nB - B);
		dC = (nC - C);

		if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }

		//Compute next iteration's values
		A = nA;
		B = nB;
		C = nC;
		A2 = A * A;
		B2 = B * B;
		C2 = C * C;
		QS = A2 + B2 + C2;
		QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
		Rsq = F0 + QB + QS;
		Q0 = 0.5f * (QS - Rsq);
		Q1 = F1 + Q0;
		Q2 = 8.0f * (QS - Rsq + QB + F0);
	}

	*sphere_x = A;
	*sphere_y = B;
	*sphere_z = C;
	*sphere_radius = sqrtf(Rsq);

	return true;
}



